#include "NavState.h"

using namespace std;

namespace gtsam {
#define TIE(R, t, v, x)     \
  const Rot3& R = (x).R_;   \
  const Point3& t = (x).t_; \
  const Vector3& v = (x).v_;

NavState NavState::Create(const Rot3& R,
                          const Point3& t,
                          const Vector3& v,
                          OptionalJacobian<9, 3> HR,
                          OptionalJacobian<9, 3> HT,
                          OptionalJacobian<9, 3> HV) {
  // dR, dP and dV in local frame

  // for rotation: R*exp(x^) = R*exp(y^) => y = x
  if (HR) {
    *HR << I_3x3, Z_3x3, Z_3x3;
  }
  // for translation: t_n = t_0 + R * dP => dP = Rt * (t_n - t_0)
  // assume t_n = t_0 ?
  if (HT) {
    *HT << Z_3x3, R.transpose(), Z_3x3;
  }
  // for velocity: v_n = v_0 + R * dV => dV = Rt * (v_n - v_0)
  // assume v_n = v_0
  if (HV) {
    *HV << Z_3x3, Z_3x3, R.transpose();
  }

  return NavState(R, t, v);
}

NavState NavState::FromPoseVelocity(const Pose3& pose,
                                    const Vector3& v,
                                    OptionalJacobian<9, 6> HPose,
                                    OptionalJacobian<9, 3> HV) {
  // dR and dP also in local frame in Pose3
  if (HPose) {
    *HPose << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
  }
  if (HV) {
    *HV << Z_3x3, Z_3x3, pose.rotation().transpose();
  }

  return NavState(pose, v);
}

const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
  if (H) {
    *H << I_3x3, Z_3x3, Z_3x3;
  }
  return R_;
}

const Point3& NavState::position(OptionalJacobian<3, 9> H) const {
  // t_n = t_0 + R * dP
  if (H) {
    *H << Z_3x3, R(), Z_3x3;
  }
  return t_;
}

const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const {
  // v_n = v_0 + R * dV
  if (H) {
    *H << Z_3x3, Z_3x3, R();
  }
  return v_;
}

Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
  // v_body = Rt * v_n
  Vector3 v_b = R_.transpose() * v_;
  Matrix3 D_bv_nRb = skewSymmetric(v_b);
  Matrix3 D_bv_v = R_.transpose() * R_.matrix();
  if (H) {
    *H << D_bv_nRb, Z_3x3, D_bv_v;
  }
  return v_b;
}

Matrix7 NavState::matrix() const {
  Matrix3 R = this->R();
  Matrix7 T;
  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
  return T;
}

ostream& operator<<(ostream& os, const NavState& state) {
  os << "R: " << state.attitude() << "\n";
  os << "p: " << state.position().transpose() << "\n";
  os << "v: " << state.velocity().transpose();
  return os;
}

void NavState::print(const string& s) const {
  cout << (s.empty() ? s : s + " ") << *this << endl;
}

bool NavState::equals(const NavState& other, double tol) const {
  return R_.equals(other.R_, tol) &&
         traits<Point3>::Equals(t_, other.t_, tol) &&
         equal_with_abs_tol(v_, other.v_, tol);
}

}  // namespace gtsam
